/*
 * Copyright (C) 2017 - 2019, AEye, Inc.
 * All rights reserved.
 */
#include <cstring>

#include "spdlog/spdlog.h"

#include "RangingFrameData.h"

RangingFrameData::RangingFrameData() :
  frameId(0), seqCounter(0), seqTotal(0), isValid(false), flexFormatDataFlags(0), totalNumPoints(0), tsSec(0), tsuSec(0) {
  points = std::make_unique<aeye::AEyePoint[]>(MAX_LIDAR_POINTS);
  reset();
}

void RangingFrameData::reset() noexcept {
  isValid    = false;
  seqCounter = 0;
  seqTotal   = 1;
  frameId    = 0;
  totalNumPoints = 0;
}

namespace {
template<typename P>
inline void copyAndIncrement(P& p, const uint8_t*& data) {
  static_assert(std::is_trivially_copyable<P>(), "only valid to memcpy into trivially copyable struct");
  std::memcpy(&p, data, sizeof(P));
  data += sizeof(P);
}
}

void RangingFrameData::addPoints(const aeye::PointReturnsBody& packetBody, const uint8_t* pData, size_t numPoints) {

  static constexpr int16_t AZ_EL_SCALING_FACTOR = 10430; // (65535.0f / static_cast<float>(M_PI)/2); This converts to radians. /2 because it's signed
  static constexpr uint8_t RANGE_SCALING_FACTOR = 64; // 65535 range values / 1023m

  if (pData == nullptr) {
    spdlog::warn("Received null points array. Treating packet as empty.");
    seqCounter++;
    return;
  }

  if (seqCounter == 0 || frameId != packetBody.frameId) {
    frameId = packetBody.frameId;
    seqTotal = packetBody.seqTotal;
    isValid   = true;
    flexFormatDataFlags = 0;
    totalNumPoints     = 0;
  }

  if (!isValid) {
    spdlog::warn("Invalid frame.");
    return;
  }

  seqCounter++;
  flexFormatDataFlags = packetBody.pointReturnMask;

  const bool sendSpherical   = ((packetBody.pointReturnMask & aeye::SPHERICAL_COORDINATES) != 0);
  const bool sendUnused_1    = ((packetBody.pointReturnMask & aeye::UNUSED_1) != 0);
  const bool sendIntensity   = ((packetBody.pointReturnMask & aeye::INTENSITY) != 0);
  const bool sendUnused_8    = ((packetBody.pointReturnMask & aeye::UNUSED_8) != 0);
  const bool sendUnused_2    = ((packetBody.pointReturnMask & aeye::UNUSED_2) != 0);
  const bool sendUnused_5    = ((packetBody.pointReturnMask & aeye::UNUSED_5) != 0);
  const bool sendUnused_6    = ((packetBody.pointReturnMask & aeye::UNUSED_6) != 0);
  const bool sendUnused_7    = ((packetBody.pointReturnMask & aeye::UNUSED_7) != 0);

  if (totalNumPoints + numPoints > currentMaxPoints) {
    reallocate(numPoints);
  }

  // For each point, copy the blocks of memory over which are present in the point returns.
  // Note that we reuse the same buffer for all pointcloud data, so it should NOT be moved from.
  // Additionally, if a requested return is removed, the last bit of data will likely stay there. Users should
  // always be ensuring that they are only reading valid data.
  for (size_t i = 0; i < numPoints; ++i) {
    auto& point = points[totalNumPoints + i];
    // Unused 0, 5 and 6 are never sent through the UDP connection. Therefore, it is crucial to avoid allocating memory
    // for their outdated structures. Otherwise, the data will be parsed incorrectly and the point cloud will display wrong coordinates!

    // Process Spherical Block
    if (sendSpherical) {
      copyAndIncrement(point.pointReturnSphericalCoords, pData);

      // Compute cartesian coordinates from the received spherical coordinates
      auto azim = static_cast<float>(point.pointReturnSphericalCoords.azimuth) / AZ_EL_SCALING_FACTOR;
      auto elev = static_cast<float>(point.pointReturnSphericalCoords.elevation) / AZ_EL_SCALING_FACTOR;
      auto range = static_cast<float>(point.pointReturnSphericalCoords.radius_scaled) / RANGE_SCALING_FACTOR;
      float cosElevation = std::cos(elev);
      point.pointReturnCartesianCoords.x = range * cosElevation * std::cos(-azim);
      point.pointReturnCartesianCoords.y = range * cosElevation * std::sin(-azim);
      point.pointReturnCartesianCoords.z = range * std::sin(elev);
    }

    // Process Unused_1 Block
    if (sendUnused_1) {
      copyAndIncrement(point.pointReturnUnused_1, pData);
    }

    // Process Intensity Block
    if (sendIntensity) {
      copyAndIncrement(point.pointReturnIntensity, pData);
    }

    // Process Unused_8 Block
    if (sendUnused_8) {
      copyAndIncrement(point.pointReturnUnused_8, pData);
    }

    // Process Unused_2 Block
    if (sendUnused_2) {
      copyAndIncrement(point.pointReturnUnused_2, pData);
    }

    // Process Unused_5 Block
    if (sendUnused_5) {
      copyAndIncrement(point.pointReturnUnused_5, pData);
    }

    // Process Unused_6 Block
    if (sendUnused_6) {
      copyAndIncrement(point.pointReturnUnused_6, pData);
    }

    // Process Unused_7 Block
    if (sendUnused_7) {
      copyAndIncrement(point.pointReturnUnused_7, pData);
    } else {
      copyAndIncrement(point.pointReturnType, pData);
    }
  }
  totalNumPoints += numPoints;
}

void RangingFrameData::reallocate(size_t numPoints) {
  size_t total = totalNumPoints + numPoints;
  spdlog::info("Reallocating ranging frame data from size {} by {} to {}", currentMaxPoints, numPoints, total);
  auto p2 = std::make_unique<aeye::AEyePoint[]>(total);
  std::copy(points.get(), points.get() + totalNumPoints, p2.get());
  points = std::move(p2);
  currentMaxPoints = total;
}

bool RangingFrameData::complete() const noexcept {
  return seqCounter == seqTotal;
}

unsigned int RangingFrameData::getFrameId() const noexcept {
  return frameId;
}

void RangingFrameData::setFrameId(unsigned int _frameId) noexcept {
  frameId = _frameId;
}

void RangingFrameData::setTime(unsigned int _tsSec, unsigned int _tsuSec) noexcept {
  tsSec  = _tsSec;
  tsuSec = _tsuSec;
}

unsigned short RangingFrameData::seqCount() const noexcept {
  return seqCounter;
}

unsigned short RangingFrameData::getFlexFormatDataFlags() const noexcept {
  return flexFormatDataFlags;
}
const aeye::AEyePoint *RangingFrameData::getPoints() const noexcept {
  return points.get();
}
unsigned long RangingFrameData::getTotalNumPoints() const noexcept {
  return totalNumPoints;
}
